
#include "BMI088.h"
#include "BMI088driver.h"
#include "BMI088reg.h"

#include "math.h"
#include "cmsis_os.h"
#include "stdio.h"

#include "PID_classic.h"

// 偏移值
static float gyro_offset[3] = {0};

static int Bmi088Flag[3]={0};

// 加速度低通滤波
static float accel_fliter_1[3] = {0.0f, 0.0f, 0.0f};
static float accel_fliter_2[3] = {0.0f, 0.0f, 0.0f};
static float accel_fliter_3[3] = {0.0f, 0.0f, 0.0f};
static const float fliter_num[3] = {1.929454039488895f, -0.93178349823448126f, 0.002329458745586203f};

PID_HandleTypeDef bmi088_temp_pid={
        .kp=TEMPERATURE_PID_KP,
        .ki=TEMPERATURE_PID_KI,
        .kd=TEMPERATURE_PID_KD,
        .max_i=TEMPERATURE_PID_MAX_IOUT,
        .target=33.0f
};

void bmi088_init_jxust(int flag)
{

    // 设置传感器
    while (BMI088_init())
    {
        osDelay(1);
    }
    float accel[3] = {0};
    get_BMI088_accel(accel);
    // 低通滤波数据初始化
    accel_fliter_1[0] = accel_fliter_2[0] = accel_fliter_3[0] = accel[0] * 9.807f;
    accel_fliter_1[1] = accel_fliter_2[1] = accel_fliter_3[1] = accel[1] * 9.807f;
    accel_fliter_1[2] = accel_fliter_2[2] = accel_fliter_3[2] = accel[2] * 9.807f;
}
void bmi088_read_data_accel(float accel[3],float temp[1]){

    BMI088_readaccel(accel);

    float tempor;
    // 右手系
    tempor = accel[0];
    accel[0] = accel[1];
    accel[1] = tempor;
    accel[2] = accel[2];

    // 加速度计低通滤波

    accel_fliter_1[0] = accel_fliter_2[0];
    accel_fliter_2[0] = accel_fliter_3[0];

    accel_fliter_3[0] =
            accel_fliter_2[0] * fliter_num[0] + accel_fliter_1[0] * fliter_num[1] + accel[0] * fliter_num[2];

    accel_fliter_1[1] = accel_fliter_2[1];
    accel_fliter_2[1] = accel_fliter_3[1];

    accel_fliter_3[1] =
            accel_fliter_2[1] * fliter_num[0] + accel_fliter_1[1] * fliter_num[1] + accel[1] * fliter_num[2];

    accel_fliter_1[2] = accel_fliter_2[2];
    accel_fliter_2[2] = accel_fliter_3[2];

    accel_fliter_3[2] =
            accel_fliter_2[2] * fliter_num[0] + accel_fliter_1[2] * fliter_num[1] + accel[2] * fliter_num[2];

    accel[0] = accel_fliter_3[0];
    accel[1] = accel_fliter_3[1];
    accel[2] = accel_fliter_3[2];


    //官方在读取bmi088加速度计数据同时也读取了温度数据 这里是保证实时控制
    BMI088_temp_control(temp[1]);
}

void bmi088_read_data_gyro(float gyro[3],float temp[1])
{

    BMI088_readgyro(gyro);

    float tempor;
    // 右手系
    tempor = gyro[0];
    gyro[0] = gyro[1];
    gyro[1] = tempor;
    gyro[2] = gyro[2];


    BMI088_temp_control(temp[1]);
}



// TODO 温度控制用官方那种形式的,将计算过程放到程序运行一段时间后再计算
void BMI088_temp_init(void)
{
    float temperature = 0;
    uint8_t count = 0;
    // 温控计时
    HAL_TIM_PWM_Start(&htim10, TIM_CHANNEL_1);

    // 两种加热方案 机器启动时等待加热 机器运行中直到某个标准量实现后记录
    //  有一部分任务属于要在系统启动之前开启并消耗一定量时间实现的(不能保证在几MS内加热完毕有一定过程
    // 实际测试当中,无论将KP调得多大,都需要消耗一定"可直观感觉"的时间,(哪怕是给它十万的KP也不行

    //! 方案一
    // while (1)
    // {
    //     temperature = get_BMI088_temperate();
    //     if (fabs(temperature - 35.0f) < 0.2f && count < 60)
    //     {
    //         count++;
    //     }
    //     else if (count >= 60)
    //     {
    //         break;
    //     }
    //     BMI088_temp_control(temperature);
    // }

    //! 方案二
    //直接在imu.c代码里直接设置对应温度标志关卡
}

void BMI088_temp_control(float temp)
{

    float out = PID_Update(&bmi088_temp_pid, temp, 1.0f);

    if (out < 0.0f)
    {
        out = 0.0f;
    }
    else if (out > HTIM10_PERCENTMAX)
    {
        out = HTIM10_PERCENTMAX;
    }
    IMU_temp_PWM(out);
}
